/*
 * @Author: your name
 * @Date: 2020-08-25 00:08:48
 * @LastEditTime: 2021-08-14 09:16:54
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \Motor024\Src\motorb.c
 */sss

aaaa
#include "location.h"
#include "rtthread.h"
#include "stm32f0xx.h"
#include "main.h"
#include "motorb.h"
#include "uart.h"
MOTOR_STRUCT motor;
//struct  rt_semaphore  mb_sem ;//motor big sem


/**
 * @description: motor back to location sense
 * @param {type} 
 * @return {type} 0: OK, 1:time out
 */
rt_uint8_t move2right(void)
{
    rt_uint16_t timeout = 0;
    rt_uint16_t delaytime = 50;
//    static rt_uint8_t move_flag = 0;
//    move_flag = 0;
    motor2_back();
    while (1)
    {
//        if(move_flag == 0)
        {
//            move_flag = 1;
           
        }
//        timeout++;
//        if(timeout == 5000/delaytime)
//        {
//            motor2_close();
//            return 1;
//        }
        if(READ_L3_BACK==0)
        {
            motor2_close();
//            KEY_FLAG = KEY_STOP;
            rt_thread_mdelay(5);
            return 0;
        }
        rt_thread_mdelay(1);
    }
    
}


/**
 * @description: 电机向前运动时间，档位设置，sel传入流量设置，一二三四五档(0.4，0.8，1.5，2.2，2.6)/5
 * @param {type} 
 * @return {type} 0:finish motion. 1 touch left location sense
 */

rt_uint8_t motorb_run_delay(rt_uint8_t is_forward, rt_uint8_t sel)
{
    rt_uint16_t cnt = 0,i = 0;
    rt_uint8_t flag = 3;
    switch (sel)
    {
    case 1:
        cnt = 400/5;
        break;
    case 2:
        cnt = 800/5;
        break;
    case 3:
        cnt = 1500/5;
        break;
    case 4:
        cnt = 2200/5;
        break;
    case 5:
        cnt = 2600/5;
        break;
    default:
        cnt = 400/5;
        break;
    }
    cnt = cnt/5;
    if(is_forward)
        motor2_move();
    else
    {
        motor2_back();
    }
    

    motor2_close();
    return 0;
}

/**
 * @description: 电机向前运动后停止时长，档位设置，sel传入速度设置， 一档1.5,2挡1.2
 * @param {type} 
 * @return {type} 
 */
rt_uint8_t motorb_stop_delay(rt_uint8_t sel)
{
    rt_uint16_t cnt = 0,i = 0;

    switch (sel)
    {
    case 1:
        cnt = 1500/10;
        break;
    case 2:
        cnt = 1200/10;
        break;
    case 3:
        cnt = 900/10;
        break;
    case 4:
        cnt = 600/10;
        break;
    case 5:
        cnt = 300/10;
        break;
    default:
        cnt = 1500/10;
        break;
    }
    for(i=0;i<cnt;i++)
    {
        rt_thread_mdelay(10);
        if(KEY_FLAG == NULL)
            return 0x1;
    }
    return 0;
}
/**
 * @description: init motor io
 * @param {type} 
 * @return {type} 
 */

//PA3 M+_IN
//PA4 M-+IN
//PA7 M+ OUT
//PB1 M- OUT
void motor_io_init(void)
{
    //PA5 motor show down
    //pA0 PA1 FI BI
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOF_CLK_ENABLE();
    
    /*Configure GPIO pin Output Level */
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_1, GPIO_PIN_SET);

    //OUT
    GPIO_InitStruct.Pin =   GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    
    GPIO_InitStruct.Pin =   GPIO_PIN_1;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
    
    /*LED*/
    GPIO_InitStruct.Pin =   GPIO_PIN_1;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);
    
    //IN
    GPIO_InitStruct.Pin =   GPIO_PIN_3|GPIO_PIN_4;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
		
    //for another debug board
    GPIO_InitStruct.Pin =   GPIO_PIN_0|GPIO_PIN_1;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

///**
// * This function will delay for some us.
// *
// * @param us the delay time of us
// */
//void rt_hw_us_delay(rt_uint32_t us)
//{
//    rt_uint32_t start, now, delta, reload, us_tick;
//    start = SysTick->VAL;
//    reload = SysTick->LOAD;
//    us_tick = SystemCoreClock / 1000000UL;//比如系统时钟频率是80M 那么1us 需要80个周期
//    do {
//        now = SysTick->VAL;
//        delta = start > now ? start - now : reload + start - now;//是倒数 还是正数
//    } while(delta < us_tick * us);
//}

void motor2_move(void)
{
    MORTOR2_FI(1);
    MORTOR2_BI(0);
	
		MORTOR2_FI_2(1);
    MORTOR2_BI_2(0);
}
void motor2_back(void)
{
    MORTOR2_FI(0);
    MORTOR2_BI(1);

    MORTOR2_FI_2(0);
    MORTOR2_BI_2(1);
}
void motor2_close(void)
{
    MORTOR2_FI(0);
    MORTOR2_BI(0);
	
    MORTOR2_FI_2(0);
    MORTOR2_BI_2(0);
}
rt_uint8_t START_KEY_G = 0;//0:stand by
//1:start
//2:stop
#define THREAD_MOTOR_B_PRIORITY         4
#define THREAD_MOTOR_B_STACK_SIZE       200

ALIGN(RT_ALIGN_SIZE)
static rt_uint8_t thread_motor_b_stack[THREAD_MOTOR_B_STACK_SIZE];
static struct rt_thread tid_motor_b;

static void thread_motor_b_entry(void *parameter)
{
    rt_uint16_t bk = 0;
    while (1)
    {
        if(KEY_FLAG == KEY_FOWARD)
        {
           
            // MORTOR_SD(0);//active motor vcc
            if(START_KEY_G == 0)
            {
//                 motor2_move();
                bk = motorb_run(1);
            }
            else
            {
                bk = motorb_run_delay(1,motor.flow);
                if(bk == 1)//touch left location keymove to righ location key
                {
                    KEY_FLAG = NULL;
                }
                else
                {
                    motorb_stop_delay(motor.speed);
                }
            }
        }
        else if(KEY_FLAG == KEY_STOP)
        {
            KEY_FLAG = NULL;
            motor2_close();
        }
        else if(KEY_FLAG ==KEY_BACK)
        {
            if(START_KEY_G == 0)
            {
                bk = motorb_run(0);
            }
            else
            {
                bk = motorb_run_delay(0,motor.flow);
                if(bk == 1)//touch righ location keymove to righ location key
                {
                    KEY_FLAG = NULL;
                }
                else
                {
                    motorb_stop_delay(motor.speed);
                }
            }
        }
        rt_thread_mdelay(1);
    }
}

/* 线程示例 */ 
int thread_motor_b(void)
{ 
    /* 静态创建线程 */
   
    /* 初始化线程 1，名称是 thread1，入口是 thread1_entry*/
    rt_thread_init(&tid_motor_b,
                   "td_mb",
                   thread_motor_b_entry,
                   RT_NULL,
                   thread_motor_b_stack,
                   THREAD_MOTOR_B_STACK_SIZE,
                   THREAD_MOTOR_B_PRIORITY,
                   50);
        /* 启动线程 */
        rt_thread_startup(&tid_motor_b);
    return 0;
}